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Motivation 





* Robotics present challenging 
environments for realizing 
estimation, perception, and 
SLAMI 


。 Visual-inertial navigation systems 
(VINS) are crucial and vvell suited 
for many applications 
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Significance of VINS 


* Improving the computational efficiency, 
robustness, and accuracy are 
challenging open research problems 


。 Improvements directly impact all 
applications which leverage VINS 


。 Directly allow for: 


1. Low-cost: sensors, compute platforms, 
and robots 


2. Reduced computational energy (longer 
missions) 





3. Better robustness and accuracy opens 
gateway for more demanding scenarios 


Outline - Estimation Methodology 


e Multi-State Constraint Kalman Filter (MSCKF) 
¢ Batch Least Squares (BLS) with Pre-integration 
。 Incremental Optimization (square-root information form) 


Problem: Visual-Inertial Estimation 
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Problem: Visual-Inertial Estimation 
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e Given: Bearings Zk-4:x and inertial x, =[gT Epl SL bi pry! 
I Gd Pr, Ik W k Ak 
readings ua -iT GT Ik-a-T GOT 1T 

XC = |G q Pr, “ae 4 Pr. | 

* Goal: Estimate inertial states and GT 


features x: = [x/ xë xj] 


Models: Inertial Kinematics 


IMU Measurement Model: 
e Gyroscope: wm = w + by + ny 
e Accelerometer: am =a+¿R“g+b, + n, 


Gj : quaternion 

: rotation matrix 

: position 

: velocity 

: angular velocity 

: linear acceleration 





: £yro. bias 

: accel. bias 

: global gravity 

: gyro white noise 

: gyro random walk 
: accel white noise 

: accel random walk 





Models: Inertial Kinematics 


IMU Measurement Model: 
e Gyroscope: wm = w + by + ny 
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Gj 


: quaternion 
: rotation matrix 





State Evolution Model: 


: position 
1 : velocity 
LE a Continuous-Time 
t) = —Muwnm(t) — blt) —n,(t t | 
cat) 9 (Wn (t) w(t) w(t) )e@q(t) State Evolution Model : angular velocity 
G. G | : linear acceleration 
pr(t) = “vr(t) X1 (t) = fo(x1(t), u(t), n(t)) : gyro. bias 
Gr I (aT G _ T ba : accel. bias 
vi(t) E R(cGq(t)) (am (t) o ba (t) — Ha (t)) — B u(t) — [kom (t) am(t)) : global gravity 
be (t) = nup : gyro white noise 
l : gyro random walk 
ba(t) = Nya : accel white noise 


‘accel random walk 





Models: Inertial Kinematics 


IMU Measurement Model: 
e Gyroscope: wm = w + by + ny 
e Accelerometer: am =a+¿R“g+b, + n, 


Gj 


State Evolution Model: 


GO = 5 Ment) — bolt) - ne) Ka Saha 
Cpr(t) = “vr(t) x(t) = f(x; (t), u(t), n(t) 
Cvi(t) = R(Gq(t)) (am(t) — ba(t) — na(t)) — g u(t) = [om(t) an) 

be(t) = Hww i 

ba(t) = Nya Discrete State 


Evolution Model 


Xk = fal(X-1, Uk—15 Wk-1) 





: Quaternion 

: rotation matrix 

: position 

: velocity 

: angular velocity 
“linear acceleration 


: gyro. bias 
ba : accel. bias 
: global gravity 
: gyro white noise 
: gyro random walk 
: accel white noise 
: accel random walk 
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Models: Camera Measurements 


* Distort to “raw” uv from the |I LE | 
"ideal" image plane uv HE LLL 
Zm — halZn, Ç) + pis | || Ll 


distort Zn to zin the raw image 
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e Project onto the “deal” distort Z to zin the raw image 
image plane {o} -— 
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Models: Camera Measurements 


* Distort to “raw” uv from the VT IT / 
“ideal” image plane uv HE LLL 
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* Project onto the "ideal" 





distort Z to z 1n the raw image 





image plane 
ui Cy JC Gj ganse 
Zn =h,(“py) = vj) 2j Fp po 
TL p T Cyr/C zy {ZR ja MI * 
G “O Pr) 
* From global ^p; to camera (1) > po 
frame ^p, z 


Cp; — h;(*p;, oR, “pr, CR, “pr) 
= TRGR(^p; — pr) + “Pi 


ec 
G to I transformation 


[1] Geneva, Patrick, et al. "OpenVINS: A research platform for visual-inertial estimation." 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020. 
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Models: Camera Measurements 


e Distort to “raw” uv from the 
“ideal” image plane uv 
£m — ha(Zn, Ç) ES Nix 
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distort Z to z 1n the raw image 


* Project onto the "idea 








image plane 
sep Ge {G} S y CI Intrinsics 
x z YA | Ç 
Zin — h,(“py) E e : C A CR, P pe 
I 

* From global ^p; to camera ine Cp, 

frame “py a E 
Cp; — h (pr, ^R, “pr, CR, C pr) (01 si E qu 2 - 


Ow Tur G C ¡ Fully combined feature measurement function: 
— RGR( Pf 一 pr) + D; ~, | Zm = h(x;,) 4 Dis 
= yy- A 
G to | transformation - 
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Background - Extended Kalman Filter (EKF) 
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prior control input / state evolution 


Background - Extended Kalman Filter (EKF) 


Propagation Update 
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Background - Extended Kalman Filter (EKF) 


Propagation Update 
diia isi J Fe 一 "一 "一 "一 "一 "一 "一 "一 "一 "一 "一 "1 
Ui = (Win, k-1 am] | | ^O DO | 
Xp 0 AN (X Pk-1) X = f Xp ~N (Xp, PE) Zm ShÇu)tË . 
| k—l k—1:^ kel) Xk = d(Xk-1, Uk—i, Wii) | [ propagation state observation | 
i prior control input / state evolution 
| | A | 
i ^O DO 
| : xp ~ N(x, PC) i 
' Xp c AN (X5, Py) 3 uidi. 
ERE ME TEE | 


Linearization: 
Zm = h(&;) + H,x+n 
ry, = HX +n 
n ~ N (0, Rx) 


Background - Extended Kalman Filter (EKF) 


T Oo. 
Uk—l = [wm k—1 am k—1| | | Xp ~ N (X7, P?) 


Xie ~ N (Xy 4, Pr—1) Xk = fa(Xk—1, Uk—1, Wk—1) | 


prior control input / state evolution 


| propagation state 


Reenen 1 IT 5 
_ Op! . 
state: i — dx + Kyr Sk = Heb Hy + Re | 

where: Ky = PH, S7? aem q 


covariance: p — (I — K,H,)P; 


Zh = h(x) + n 


observation [ 


Linearization: 


Zm = h(&;) + Hx +n 
ry = HX +n 
n ~ N (0, Rg) 


Multi-State Constraint Kalman Filter (MSCKF) 


e The MSCKF allows for updating features without inserting their 
estimates into the state vector x, = [x xj] 


* Reduced complexity increases computational efficiency 
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MSCKF Measurement Update 


* Measurement equations 
41 = h(xz, Pri) 十 Moix 


22 = h(xz, Pri) T Moix = 
ab 





MSCKF Measurement Update 


。 Measurement equations 
zi = h(xx, pri) + nyi 
Z2 = h(x, Pri) T is 

* Linearized measurement equation 
215 = HX, + Hy Dy, + Mpio 





MSCKF Measurement Update 


* Measurement equations 
41 = h(x, Pri) 十 Moix 





Z2 = h(x;, Pri) T Apia E (CC 
* Linearized measurement equation e) tk) y 
Zio = H, X, + Hye “Py, + Mpio ni 
* Project onto Hy left nullspace Q, Ha 





"= Equivalent to marginalization 
On Zio 2 On His + Qu | Hy“ py, + Qu Mpio 


Qu Zio = Qu H,X, + Qu nj 


MSCKF Measurement Update 


* Measurement equations 
41 = h(x, Pri) 十 Moix 


22 一 h(xr, “Pri) Ea Moix KA 
。 Linearized measurement equation y 
: L 
Z1:2 = Hx; Eu H;^p;, T Nix By 
nullspace 
e Project onto H; left nulispace q, prona 





“ Equivalent to marginalization 


On Zio = On His T Qu | Hy“ py, + Qu Mpio 


E T ~ T arti — 
Qn 21-2 = On Hx, + On nj, t — Nullspace projection causes derivative in 
respect to the feature to go to zero! 


MSCKF Measurement Update 


* Measurement equations 
41 = h(x, Pri) 十 Moix 


22 一 h(xr, Cpr) AP Moix KA 
。 Linearized measurement equation y 
— ~ dl » 4 
Z1:2 = H,xy, + H;^p;, T yi; DU 
nullspace 
e Project onto H; left nulispace q, prona 





“ Equivalent to marginalization 


Qu Zi: 一 Qu H,X, 十 Qu H;^p;, 十 Qu nj 


To T ~ T VU MP 
Qn Zi2= On Hex, + Qu D yia t — Nullspace projection causes derivative in 
respect to the feature to go to zero! 


* Perform standard EKF update! 


MSCKF — Estimation Flovvchart: Recap 


。 MSCKF is an efficient method Step 1 Step 2 
for light-weight sensor fusion 


。 Two step process: Inertial Propagate 
propagation and measurement 
update 


Track Sparse Nullspace 
Features Projection 
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MSCKF — Estimation Flovvchart: Recap 


。 MSCKF is an efficient method Step 1 Step 2 
for light-weight sensor fusion 


。 Two step process: Inertial 
propagation and measurement 
update 





Track Sparse Nullspace 
Features Projection 
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Problem: Factor Graph Perspective 


Robotic Trajectory: 





Problem: Factor Graph Perspective 


* Robotic Trajectory: 






Nikon 
> 
miis añ 
M MAIN 


Converted 


to Factor 
e Factor Graph Form: G Graph 
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Batch Least Squares (BLS) Estimation 


Prior Cost Inertial Cost Feature / Bearing Cost 
『 E EE nnn =. um OH SE em pm s ee ee 
B ( ) | h(x:) +n 
/ i Xk = fa Xk— 1, Uķk—1, Nk—1 s d ëm = Xk 

| Xo = Xy +n | Ñ | C observation 

| |  . control input / state evolution i 

| y | + | 

| C | M2 = 2 

: rior — ||X0 — xollp i 2 l © Ccam — |Zm = h(x%) 118 

Ba 0 l Cimu = ||Xk — fa(Xk-1, uk-i;nk—i)lo, . 1 i 

-— = ooo. ama mad L BE EE PO O ë NONEM O No NEM O ESO O NEN J ms Bg E nnn ë 
[1] Grisetti, Giorgio, et al. "g20: A general framework for (hyper) graph optimization." Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China. 2011. 14 


[2] Dellaert, Frank. Factor graphs and GTSAM: A hands-on introduction. Georgia Institute of Technology, 2012. 


Batch Least Squares (BLS) Estimation 


Prior Cost Inertial Cost Feature / Bearing Cost 
|] O o o =. um a = | E m m — EM nnn RO oo Rn Am q mm n umm RB REM E O RH wm =. ums OH 
b. — f Uj Zh = h(x) + n 
| / Xk = d(Xk—1, Uk—1, Mp1) i m k 
Xo = Xq TN | observation 
| I, control input / state evolution 
y , | y P 
| Cprior = [Xo — xoll, ; ! 1 © Cam = ||Zm — h(xx)|I%, 
| € Cimu = ||Xk — fa(Xk-1, Ur, De DIO, . 1 
nnn ama =l L E EE PO O ë O NONEM O O O O EN J - sg E NE NEM i No NEN e NEM O e O NEN 
Optimization Formulation 
| a 
i k | 
| oT Tal > | ! - 
| xo ; X1 pe XL | = = argmin Cprior 十 eS. T CH, | L Can use Gauss- 
' Ko..k ;一 0 Newton algorithm 
i | 
[1] Grisetti, Giorgio, et al. "g20: A general framework for (hyper) graph optimization." Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China. 2011. 14 


[2] Dellaert, Frank. Factor graphs and GTSAM: A hands-on introduction. Georgia Institute of Technology, 2012. 


BLS — Efficient Estimators 


Challenge: 


= Standard BLS is cubic in terms of the state size O(n?) 
= Application to real-time robotic systems requires modifications 


Solutions: mE 
“ Sliding window optimization 


(fixed-lag smoothers) jan am ck < 
— 
= BLS with graph reduction “Po pn | AR | 
| 
| 


(sparsification) mE pi 
x X x X xX 
" Incremental optimization T2222 O i | ? 
t. States we wish to marginalize to t. States are now 
reduce state size & save compute. related through prior. 


[1] Leutenegger, Stefan, et al. "Keyframe-based visual-inertial odometry using nonlinear optimization." The International Journal of Robotics Research 34.3 (2015): 314-334. 
[2] Kaess, Michael, et al. "iSAM2: Incremental smoothing and mapping using the Bayes tree." The International Journal of Robotics Research 31.2 (2012): 216-235. 
[3] Hsiung, Jerry, et al. "Information sparsification in visual-inertial odometry." 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018. 
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BLS — Marginalization 


* Many different practical methods to enable efficient BLS 


。 Key Considerations: 
= What states to marginalize? 
" When to marginalize? 


* Examples: 
= Keyframing of poses 
= Dropping of features 
" Duplication of measurements 
= Non-linear factor recovery 
" Fully dense marginal factor 





Temporal/IMU window 
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[photo] Leutenegger, Stefan, et al. "Keyframe-based visual-inertial odometry using nonlinear optimization." The International Journal of Robotics Research 34.3 (2015): 314-334. 
[2] Nerurkar, Esha D., Kejian J. Wu, and Stergios |. Roumeliotis. "C-KLAM: Constrained keyframe-based localization and mapping." 2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014. 


BLS — Inertial Pre-integration 


e Naive use of inertial measurement model requires re-integration of Cimu 


* Decouple integration and state variables (through approximations) to 
remove need to re-integrate when states re-linearize during optimization 


Not a function of current state 


k+1 +1). (after bias linear approx)! 


Tk+1 
G G G T G k u 7, 
Vk+1 — Vk m gAT 十 k a / ,R( Am m ba m Na) du 
tk 


Cinu = IX = fa(Xk—1, Uri "kB, 





^ k-11 
"F-.. t et E 
t y i A t 
E A : A 5 "Prr1= “ppt Vv, AT 一 B 'e£AT" +; / / SR (“am — ba — na) duds 
TI t4 E bk lk 





17 


[1] Forster, Christian, et al. "On-manifold preintegration theory for fast and accurate visual-inertial navigation." IEEE Transactions on Robotics (2015): 1-18. 
[2] Eckenhoff, Kevin, Patrick Geneva, and Guoquan Huang. "Closed-form preintegration methods for graph-based visual—inertial navigation." The International Journal of Robotics Research 38.5 (2019): 563-586. 


BLS — Estimation Flovvchart: Recap 


Factor graphs can provide 
a unified design 
language for sensor Camera — | Track Sparse 


Reprojection 





| | Ccam 
fusion algorithms — 
e Allows for higher level 
A | => 
abstraction away from IMU => rwn — | Factor Graph | . | Marginalize 
linear algebra “as 
representation y ft 


Step 1: Append Factors 
Step 2: Optimize 
Step 3: Marginalize 


Optimization 


Solver 





Incremental Optimization using Sart-Info 


。 Key ideas: 
= Use Cholesky factorization of Hessian (information) 
" Can formulate equivalent optimization problem using square-root matrix 


* Advantages of square-root form: 


= Better system numerical properties (smaller condition number) can enable use of 
single-precision arithmetic 


" Optimal state ordering can allow for efficient Givens QR (e.g. ISAM) 


square-root information Reformulate original BLS in square- 


Standard BLS solution l through QR decomposition root form (equivalent) 
argmin|H(x ^x)—r|^ (1) H = QU : Trl? 
HTHAx = Hr «> U'UAx-U'Q'r 4» arsmin[[U(x-x)-Q ri" (2) 


UAx =Q'r 


[1] Dellaert, Frank, and Michael Kaess. "Square Root SAM: Simultaneous localization and mapping via square root information smoothing." The International Journal of Robotics Research 25.12 (2006): 1181-1203. 

[2] Maybeck, Peter S. Stochastic models, estimation, and control, vol 1. Academic press, 1982. 

[3] Kaess, Michael, Ananth Ranganathan, and Frank Dellaert. "iSAM: Incremental smoothing and mapping." IEEE Transactions on Robotics 24.6 (2008): 1365-1378. 19 
[4] Wu, Kejian, et al. "A Square Root Inverse Filter for Efficient Vision-aided Inertial Navigation on Mobile Devices." Robotics: Science and Systems. Vol. 2. 2015. 


Incremental Optimization — An Example 





Prior Cost 


KERR om Gri UP 
ma eee | 


。 All information stored in sart 
information form P*=U'U 


[1] Dellaert, Frank, and Michael Kaess. "Square Root SAM: Simultaneous localization and mapping via square root information smoothing." The International Journal of Robotics Research 25.12 (2006): 1181-1203. 
[2] Wu, Kejian, et al. "A Square Root Inverse Filter for Efficient Vision-aided Inertial Navigation on Mobile Devices." Robotics: Science and Systems. Vol. 2. 2015. 
[3] Golub, Gene H., and Charles F. Van Loan. Matrix computations. Vol. 3. JHU press, 2013. 
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Incremental Optimization — An Example 


Prior Cost 


。 All information stored in sart 
information form P^ -U'U 


Inertial & Feature Cost 


i " -——- E py — = RRUG | E --— no mmm no mm no EM no Em ou | New Factors: 

XL = Xp—1, Up—_1, NE - A : A 
a OO | = Will ruin upper triangle structure 

Lim — X n i i : 
|o 707 Ox) + | = Re-triangulate using QR 

A 2 | " Appends new information to sqrt matrix! 

|, Cimu/cam = ||H(x — X) — r|| | Pp | q | 
| | = State re-ordering allows for efficient QR 
[1] Dellaert, Frank, and Michael Kaess. "Square Root SAM: Simultaneous localization and mapping via square root information smoothing." The International Journal of Robotics Research 25.12 (2006): 1181-1203. 20 
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Incremental Optimization — An Example 


Prior Cost 


。 All information stored in sart 
information form P^! -U'U 


Inertial & Feature Cost 





i " -——- E py — = RRUG | E --— no mmm no mm no EM no Em ou | New Factors: 

XL = Xp—1, Up—_1, NE - A : A 
a OO | = Will ruin upper triangle structure 

Lim — X n i i : 
|o 707 Ox) + | = Re-triangulate using QR 

A 2 | " Appends new information to sqrt matrix! 

|, Cimu/cam = ||H(x — X) — r|| | Pp | q | 
| | = State re-ordering allows for efficient QR 
[1] Dellaert, Frank, and Michael Kaess. "Square Root SAM: Simultaneous localization and mapping via square root information smoothing." The International Journal of Robotics Research 25.12 (2006): 1181-1203. 20 


[2] Wu, Kejian, et al. "A Square Root Inverse Filter for Efficient Vision-aided Inertial Navigation on Mobile Devices." Robotics: Science and Systems. Vol. 2. 2015. 
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Estimation Methodology Equivalences 


EKF <> MAP Optimization w/ one Gauss-Newton Iteration 
EKF S Extended Inverse (Information) Filter (EIF) 
EKF © Square-Root EKF (SW-EKF) 
MSCKF (nullspace) & BLS with Feature Marginalization (schur) 


i Takeaway: 


| | | a | | 
‚Equivalent up to linearization errors in theory. Choice 
iis based on end application use (e.g., computational 
efficiency and accuracy levels required). 


[1] Bell, Bradley M., and Frederick VV. Cathey. “The iterated Kalman filter update as a Gauss-Newton method.” IEEE Transactions on Automatic Control 38.2 (1993): 294-297. 
[2] Wu, Kejian, et al. "A Square Root Inverse Filter for Efficient Vision-aided Inertial Navigation on Mobile Devices." Robotics: Science and Systems. Vol. 2. 2015. 
[3] Yang, Yulin, James Maley, and Guoquan Huang. "Null-space-based marginalization: Analysis and algorithm." 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017. 


Outline — Essential Building Blocks 


* Feature Tracking and Matching 
* Observability 

* Filter Consistency 

* Degenerate Motion 

* |nitialization 

e Calibration 

* Robustness 

* Long-Term Navigation 


Feature Tracking and Matchine 





。 Indirect methods 
" Geometric points or lines 


= Track temporally using KLT or 
descriptors 


= RANSAC for outlier rejection 


e Direct methods 
“ Intensity based cost function 





= Still rely on gradient 
information, but more robust 
in low-texture environments 


" How to project features into 
future frames? 


[1] Lucas, Bruce D., and Takeo Kanade. "An iterative image registration technique with an application to stereo vision." 1981. 

[2] Rublee, Ethan, et al. "ORB: An efficient alternative to SIFT or SURF." 2011 International conference on computer vision. leee, 2011. 
[3] Golub, Gene H., and Charles F. Van Loan. Matrix computations. Vol. 3. JHU press, 2013. 

[4] Bloesch, Michael, et al. "Iterated extended Kalman filter based visual-inertial odometry using direct photometric feedback." The International Journal of Robotics Research 36.10 (2017): 1053-1072. 
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Observability 


e Determine if we are able to 
fully recover the state given 
sensor measurements 


Mi H, 

Mo HoP(o,1) 
M= |. | = | 

Mi. Hi, 1) 


" Compute by stacking all 
Jacobians and state transitions 


= |f nullspace exists then it is the 
unobs. direction MN — 0 


* Why we care about observability: 


= Determines minimal information to 
recover states 


" Enables design of consistent 
estimators (e.g., FEJ, OC-EKF, etc.) 


" Identify degenerate motions 





Conical 4 unobs. directions for VINS: 
Yaw (left), Translation (right) 


[1] Yang, Yulin, and Guoquan Huang. "Observability analysis of aided ins with heterogeneous features of points, lines, and planes." IEEE Transactions on Robotics 35.6 (2019): 1399-1418. 


Estimator Consistency 





enees = (x — X) P (x— X) 


y” NEES is large since covariance is overconfident 


e Estimation error should be zero 
mean and estimate covariance 





should be larger than or equal to the o EE] „AA 
true covariance : PËR NRI. 
* Preventing information gain in ji 





unobservable directions is key to 
improving consistency unm 


—0— Std-VINS —0— Std-VINS 
一 二 OC-VINS PR NR RN = —t— OC-VINS 
4 TO |deal-VINS — —E- |deal-VINS 
8 eN 


e Existing Algorithms: 
= Robot-centric 


o 
D 
> 0. 
UJ 
Ul 
z 
Œ 
c 
o 
S 
c 
o 
— 
Õ 





" First-Estimate Jacobians All preserve 
= OC-EKF se ve t. Inconsistency cause estimator errors, thus 
= Invariant filters n we minimize inconsistencies! 


[1] Castellanos, José A., José Neira, and Juan D. Tardós. "Limits to the consistency of EKF-based SLAM." IFAC Proceedings Volumes 37.8 (2004): 716-721. 

[2] Huang, Guoquan P., Anastasios |. Mourikis, and Stergios |. Roumeliotis. "Observability-based rules for designing consistent EKF SLAM estimators." The International Journal of Robotics Research 29.5 (2010): 502-528. 

[3] Wu, Kanzhi, et al. "An invariant-EKF VINS algorithm for improving consistency." 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017. 25 
[4] Bar-Shalom, Yaakov, X. Rong Li, and Thiagalingam Kirubarajan. Estimation with applications to tracking and navigation: theory algorithms and software. John Wiley & Sons, 2004. 

[photo] Hesch, Joel A., et al. "Camera-IMU-based localization: Observability analysis and consistency improvement." The International Journal of Robotics Research 33.1 (2014): 182-201. 


Degeneracy 


Situations which cause additional 
unobservable directions in VINS 
(4DoF canonical) 


Identify degeneracies through 
inspection of observability matrix 
“ System — Likely to cause VINS to fail 


" Calibration — Can possibly degrade 
performance 


* Degeneracies weaken system 
robustness 


。 Degeneracies can test estimator 
consistency since there should be 
no information gained 


0 ¿MU Gyro Intrinsic (1:3) Error 


System Degeneracies 


Sensor — | Unohservable 
l. Pure translation Orientation LR 
S 





Calibration Degeneracies 





0.07 ¿MU Gyro Intrinsic (4:6) Error 


t. No information is gained in unobservable 
directions (constant variance) 
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[1] Yang, Yulin, et al. "Degenerate motion analysis for aided ins with online spatial and temporal sensor calibration." IEEE Robotics and Automation Letters 4.2 (2019): 2070-2077. 
[2] Yang, Yulin, et al. "Online IMU Intrinsic Calibration: Is It Necessary?." Proc. of the Robotics: Science and Systems, Corvallis, Oregon (2020): 716-20. 


State Initialization 


。 Initialization is the task of 


AN Mp i T 
determining the initial system state x, = (nj Go] Gy] bi, bij 
e VINS has 4DoF unobservable, thus t. Global yaw and position unobservable 


(thus can be chosen arbitrarily) 


need to initialize the other 11DoF 


* Initialization Challenges: Example SFM Procedure: | 
= Want to initialize as fast and robustly | 1. Collect a window of measurements | 
as possible 2. Perform traditional visual SFM to get up- ! 

= Sh . k h | to-scale camera trajectory 
lorter time ma es recovering the 3. With pre-integrated inertial readings 
initial states more difficult or unobs. l recover gravity and state velocities l 
" Longer times introduce error due to 14. Refine estimates with non-linear I 
optimization to get final state estimates | 


time offsets, inertial noise, along with L 
increased computation 


[1] Martinelli, Agostino. "Closed-form solution of visual-inertial structure from motion." International journal of computer vision 106.2 (2014): 138-152. 
[2] Dong-Si, Tue-Cuong, and Anastasios |. Mourikis. "Estimator initialization in vision-aided inertial navigation with unknown camera-IMU calibration." 2012 IROS. IEEE, 2012. 
[3] Campos, Carlos, José MM Montiel, and Juan D. Tardós. "Inertial-Only Optimization for Visual-Inertial Initialization." 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020. 
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Calibration 


* Can be performed offline prior to 


estimation, online during, or both 


* Offline calibration: 
* Highly accurate 
e Can control sensor motion 
* Might not always be possible 


e Online calibration: 


* Crucial for practical deployments 
handling of poor initial values 


* Handling time-varying calibration 
parameters 


* Improves estimation robustness 


Intrinsics 


, C 
iG] EN C 
KRIP Ju 


I Example Parameters 

| Camera-inertial spatial-temporal 

| Camera intrinsic (focal, center, dist) 
| Inertial intrinsic (scale, skew) 


Robustness and Resiliency 


* Challenges: 
1. Hard failures — no measurement 
information (sensor drop) 


2. Soft failures — data becomes corrupt 
(invalidated measurement model) CAMZ MII-100 CAMI CAMO 


H ho 

DË k P a — GA —- NN ==; 
E A = ”一 一 一 B— — -一 一 一 
— ; —— == | 











* Examples: 


" Unmeasurable external forces (e.g., 
moving platform) 


" Dynamic environments 


" Sensor Va riations (e.g., exposure, 3 Stereo Rolling-Shutter Cameras (only left of each used) 
- 640x480 ELP-960P2CAM-V90-VC 
tem pe ratu re) - 30 Hz sensing rate 


2 Inertial Measurement Units 
- XSENS MT-100 400 Hz 


- Microstrain 3DM-GX-25 500 Hz 





* Can address through leveraging multi- 
sensor fusion 


29 
[1] Eckenhoff, Kevin, Patrick Geneva, and Guoquan Huang. "MIMC-VINS: A versatile and resilient multi-IMU multi-camera visual-inertial 
navigation system.” IEEE Transactions on Robotics (2021). 


Long-lerm Navigation 





* Can incorporate loop-closure directly 
or split the problem into two parts: 


= Frontend (Localization): Fast, drifts with 
time, short-term accuracy matters 


"= Backend (Mapping): Slow, loop-closure, 

global consistency matters —- 9 — 

SELIS Tae SS US 7/23 e3 Ce O00 (S 
Standard MSCKF w/o Loop Closures 


* Challenges: 
" Incremental vs full batch pose graph 4 
= Optimal selection problem to reduce E 
complexity and memory usage 


= Robust loop-closure detection and 
constraints 


[1] Qin, Tong, Peiliang Li, and Shaojie Shen. "Vins-mono: A robust and versatile monocular visual-inertial state estimator." IEEE Transactions on Robotics 34.4 (2018): 1004-1020. 


4 
^ 


Keyframe Mátch 1 


x40 





Addition of loop-closures can limit odometry drift 4 
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[2] Geneva, Patrick, Kevin Eckenhoff, and Guoquan Huang. "A linear-complexity EKF for visual-inertial navigation with loop closures." 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019. 


Outline - Available Open Sourced Systems 


* Open Source Estimators 
* Dataset Benchmarks 
。 Metrics and Evaluation 


Visual-Inertial Research: Embracing Open Source 


System Mono? Stereo? EKF? Indirect? Calib. Spatial? Calib. Intrinsics?  Calib. Time? 


OpenVINS W Vl 
S-MSCKF - W 
R-VIO 
Rovioli 


/ 
/ 
XIVO / 
/ 
"4 


/ / 


1 和 和 和、 和、 


VINS-Fusion 
OKVIS 
Basalt 
ICE-BA 
Kimera-VIO 
ORB-SLA M3 


SNSSKSSHKSNSOUSNSNSN 
OSNSSUNN 


AN 
NNSNSNK KSK 


e Wide range of systems available for visual-inertial research 


e Our group s: OpenVINS which is a feature complete filter system for 
use on resource constrained platforms and as an odometry frontend 
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OpenVINS 





* An open platform for VINS research 
(OpenVINS) which achieves state-of- 
the-art performance 


* On manifold sliding window Kalman 
filter with modular type system for 
state management (gtsam inspired) 


* Detailed documentation and 
derivations to support researchers 
using the codebase: 
https://docs.openvins.com/ 


[1] Geneva, P., Eckenhoff, K., Lee, W., Yang, Y. and Huang, G., 2020, November. OpenVINS: A research platform for visual-inertial estimation. In 
Proc. of the IEEE International Conference on Robotics and Automation, Paris, France. 


rpng / open vins 


ty Code Issues 1 


visual-inertial-odometry 


-Cr 225 commits 


Branch: master 一 


Pull requests O Actions Security 


An open source platform for visual-inertial navigation research. htt 


slam msckf sensor-calibration ekf-localization 


E 3 branches EN O packages (> 2 releases 


New pull request 


== goldbattle fixed - rosvisualizer not being included in shared library on build 


EH docs 

EH ov core 

EH ov data 

EH ov eval 

BH ov msckf 

E) .gitignore 

E) CMakeLists.txt 
E] Doxyfile 

E) Doxyfile-mcss 
E) LICENSE 


E) ReadMe.md 


updated readme and links to scripts in docs 
updated documentation 

updated uzh-fpv groundtruths with the new time £ 
updated documentation 

fixed - rosvisualizer not being included in shared li 
Added - Continuous preintegration classes with all 
more descriptive namespace documentation 

fixed small bug where if mono single depth didn't 
Breaking - Moved types to new namespace, addec 
added aplvs license file 


updated readme and links to scripts in docs 


https://github.com/rpng/open vins 
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OpenVINS — Key Features 


Sliding window visual-inertial MSCKF 
Modular covariance type system 


Comprehensive documentation and derivations 


Extendable visual-inertial simulator 
e On manifold SE(3) b-spline 
e Arbitrary number of cameras 
e Arbitrary sensor rate 
e Automatic feature generation 


Five different feature representations 


Environmental SLAM feature 
e OpenCV ARUCO tag SLAM features 
e Sparse feature SLAM features 


Calibration of sensor intrinsics and extrinsics 
e Camera to IMU transform 
* Camera to IMU time offset 
* Camera intrinsics 


Visual tracking support 
e Monocular / Stereo / Binocular cameras 
e KLT or descriptor based 





First-Estimate Jacobians for consistent estimation 
Static IMU initialization 
Zero velocity detection and updates 


Out of the box dataset evaluation on: 
e EurocMav 
e TUM-VI 
e UZH-FPV Drone Racing 
e KAIST Urban Driving 


Extensive evaluation suite: 
e ATE, RPE, NEES, RMSE 
e Timing evaluation and plotting 


Codebase extensions: 
* ov secondary — Secondary pose graph with loop-closure 
e ov maplab - Multi-session mapping and offline optimization 
* vicon2gt — Groundtruth gen. for VIO dataset evaluation 
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Open VINS — Key Features 





e Sliding window visual-inertial MSCKF 
e Modular covariance type system 
e Comprehensive documentation and derivations 


Is Extendable visual-inertial simulator | 
a * On manifold SE(3) b-spline i 
| e Arbitrary number of cameras 
] e Arbitrary sensor rate i 
l * Automatic feature generation | 


e Five different feature representations 


* Environmental SLAM feature 
e OpenCV ARUCO tag SLAM features 
e Sparse feature SLAM features 


e Calibration of sensor intrinsics and extrinsics 
e Camera to IMU transform 
。 Camera to IMU time offset 
e Camera intrinsics 


e Visual tracking support 
e Monocular / Stereo / Binocular cameras 
e KLT or descriptor based 


First-Estimate Jacobians for consistent estimation 
Static IMU initialization 
Zero velocity detection and updates 


Out of the box dataset evaluation on: 
e EurocMav 
e TUM-VI 
e UZH-FPV Drone Racing 
e KAIST Urban Driving 


Extensive evaluation suite: 
e ATE, RPE, NEES, RMSE 
e Timing evaluation and plotting 


Codebase extensions: 
* ov secondary — Secondary pose graph with loop-closure 
e ov maplab - Multi-session mapping and offline optimization 
* vicon2gt — Groundtruth gen. for VIO dataset evaluation 
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Open VINS — Key Features 





e Sliding window visual-inertial MSCKF 
e Modular covariance type system 
e Comprehensive documentation and derivations 


1。 Extendable visual-inertial simulator | 
a * On manifold SE(3) b-spline i 
| e Arbitrary number of cameras 
] e Arbitrary sensor rate i 
| ° Automatic feature e generation | 


e Five different feature representations 


* Environmental SLAM feature 
e OpenCV ARUCO tag SLAM features 
° Sparse feature SLAM features 


F E E Ee B 

.* Calibration of sensor intrinsics and | extrinsics! 
| * Camera to IMU transform | 
| | 


。 Camera to IMU time offset 
e Camera intrinsics 


e Visual tracking support 
e Monocular / Stereo / Binocular cameras 
e KLT or descriptor based 


First-Estimate Jacobians for consistent estimation 
Static IMU initialization 
Zero velocity detection and updates 


Out of the box dataset evaluation on: 
e EurocMav 
e TUM-VI 
e UZH-FPV Drone Racing 
e KAIST Urban Driving 


Extensive evaluation suite: 
e ATE, RPE, NEES, RMSE 
e Timing evaluation and plotting 


Codebase extensions: 
* ov secondary — Secondary pose graph with loop-closure 
e ov maplab - Multi-session mapping and offline optimization 
* vicon2gt — Groundtruth gen. for VIO dataset evaluation 
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Open VINS — Key Features 





e Sliding window visual-inertial MSCKF 。 First-Estimate Jacobians for consistent estimation 
e Modular covariance type system 。 Static IMU initialization 
e Comprehensive documentation and derivations 2 Zero velocity detection and updates 


| * Extendable visual-inertial simulator e Out of the box dataset evaluation on: 


a * On manifold SE(3) b-spline e EurocMav 

| e Arbitrary number of cameras e TUM-VI 

i e Arbitrary sensor rate j e UZH-FPV Drone Racing 
| ° Automatic feature e generation | e KAIST Urban Driving 


。 ATE, RPE, NEES, RMSE 
* Timing evaluation and plotting 


* Environmental SLAM feature 
e OpenCV ARUCO tag SLAM features 
o Sparse: feature SLAM features 。 Codebase extensions: 
* Ov secondary - Secondary pose graph with loop-closure 


* ov maplab — Multi-session mapping and offline optimization; 


e Calibration of s sensor intrinsics and extrinsics 


| 
| 
I 
。 Five different feature representations Ie Extensive evaluation suite: 
I 
E 1 | 
| * Camerato IMU transform | 
| : 


。 Camera to IMU time offset * vicon2gt — Groundtruth gen. for VIO dataset evaluation É 
* Camera intrinsics 

p Visual tracking support ul dc sd t. Lots of great features directly “out-of-the-box” 
。 Monocular / Stereo / Binocular cameras to enable research and practical deployment! 


e KLT or descriptor based 
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Wide Range of VINS Datasets 


Dataset 
EuRoC MAV 


NCLT 


TUM VI 


TUM VI RS 
UZH-FPV Drone 


KAIST Urban 


Blackbird 


Newer College Datset 
MADMAX 


NTU VIRAL 


Year 


2016 


2016 


2018 


2019 
2019 


2019 


2020 


2020 


2021 


2021 


Enviroment 
Indoor Structured 
Large-scale 
Outdoor Structured 


Indoor / Outdoor 
Structured 


Indoor Structured 
Indoor / Outdoor 
Large-scale 


Outdoor Structured 


Simulated Indoor 
Structured 
Outdoor Structured 


Outdoor Mars 
Enviroment 


Outdoor Structured 


Groundtruth 
6D Vicon 
3D Laser ‘Tracker 


RTK+SLAM 


Partial 6D Vicon 


6D Vicon 
3D Laser Tracker 


RTK+SLAM 


6D Vicon 


ICP to 
Prior Map 


9D RIK 


3D Laser Tracker 


Sync? 
Yes 


No 


Other Sensors 


LiDAR 
Ladybug 
Wheel Encoders 


Rolling and 
Global Shutter 
Event Camera 

LiDAR 
Wheel Encoders 
FOG 
Motor Tachometers 
Depth 
Segmentation 
LiDAR 
Depth 
LiDAR 
Depth 
LiDAR 
UWB 


Platform 


UAV 


Rover 


Handheld 


Handheld 


UAV 


Car 


UAV 


Handheld 


Rover 


UAV 


Wide Range of VINS Datasets 


Dataset Year 

EuRoC MAV 2016 

NCLT 2016 

TUM VI 2018 

TUM VI RS 2019 

| UZH-FPV Drone | 2019 

I KAIST Urban ! 2019 
I i 

| Blackbird | 2020 

Nevver College Datset 2020 
I i 

i MADMAX . 2021 
| | 

NTU VIRAL “2071 


Enviroment 
Indoor Structured 
Large-scale 
Outdoor Structured 


Indoor / Outdoor 
Structured 


Indoor Structured 
Indoor / Outdoor 
Large-scale 


Outdoor Structured 


Simulated Indoor 
Structured 
Outdoor Structured 


Outdoor Mars 
Enviroment 


Outdoor Structured 


Groundtruth 
6D Vicon 
3D Laser ‘Tracker 


RTK+SLAM 


Partial 6D Vicon 


6D Vicon 
3D Laser ‘Tracker 


RTK+SLAM 


6D Vicon 


ICP to 
Prior Map 


9D RIK 


3D Laser Tracker 


Sync? 
Yes 


No 


Other Sensors 


LIDAR 
Ladybug 
VVheel Encoders 


Rolling and 
Global Shutter 
Event Camera 

LiDAR 
Wheel Encoders 
FOG 
Motor Tachometers 
Depth 
Segmentation 
LiDAR 
Depth 
LiDAR 
Depth 
LiDAR 
UWB 


Platform 
UAV 


Rover 


Handheld 


Handheld 
UAV 


Car 


UAV 


Handheld 


Rover 


UAV 


Metrics tor Evaluation 


, | * Absolute Trajectory Error (ATE) 
e Quality of groundtruth remains 


challenging in realworld datasets 





。 High quality metrics allow for fair 


comparison of different algorithms 
P 5 * Relative Pose Error (RPE) [recommended] 


e nnn ee 5 nni 
Open Source Toolboxes: =- e naeia 
LE trajectory segments 
E EVO - https://github.com/MichaelGrupp/evo added " D; a" J y seg 
= rpg trajectory evaluation - Crpe d; = + > IIR — xrll 
https://github.com/uzh-rpg/rpg trajectory evaluation Dj 
E = k—l 


E] = . . i 
NJAA = ni cn e Normalized Estimation Error Squared (NEES) 
1 N 
— ^ Tp-l A 
Cnees,k = 和 2 s — Xki) Pp 5(Xki — Xii) 
¿=1 
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[1] Zhang and Scaramuzza. "A tutorial on quantitative trajectory evaluation for visual (-inertial) odometry." International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018. 


Metrics tor Evaluation 


Quality of groundtruth remains 
challenging in realvvorld datasets 


High quality metrics allovv for fair 
comparison of different algorithms 


* Open Source Toolboxes: 


E EVO - https://github.com/MichaelGrupp/evo 


" rpg trajectory evaluation - 
https://github.com/uzh- 'pg/rpg trajectory evaluation 


t Fast evaluation tool vvith additional recording 
and timing utilities! 


Absolute Trajectory Error (ATE) 





Relative Pose Error (RPE) [recommended] 


Relative pose for 
trajectory segments 


1 - 2 
D. ND |X — xrll 
* k—l 


Normalized Estimation Error Squared (NEES) 
N 
1 A _ A 
Cnees,k 一 N ) ens — Rki) Po; ESP m Rki) 


Crpe,d; — 
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[1] Zhang and Scaramuzza. "A tutorial on quantitative trajectory evaluation for visual (-inertial) odometry." International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018. 


Outline — Conclusion 


e Future Directions 


e Conclusion 


VVhere Next” 


* Practicality: Many challenges to widely deploying VINS 
(e.g. known calib etc.) 


* Robustness: Moving environment, long-term seasons, 
sensor variances 


* Semantic Understanding: Object-wise and uncertain 
network classifications 


* Computational: Real-time robotic systems (low-cost loT 


evices, latency, etc.) 


* Aided-INS: Incorporating additional sensors (e.g. event, 
thermal, etc.) 


* Cooperative: Multi-robot systems (measurement 
selection, distributed, scalability, etc.) 


* Dynamics: Integrate robot dynamics 
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[1] Huang, Guoquan. "Visual-inertial navigation: A concise review." 2019 international conference on robotics and automation (ICRA). IEEE, 2019. 
[photo] Dong, Jingming, Xiaohan Fei, and Stefano Soatto. "Visual-inertial-semantic scene representation for 3D object detection." Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2017. 


. > Cam0 MSCKF Feat = Purple 
Realtime MIMC VINS Cami MSCKF Feat = Cyan 


Robustness to Cam2 MSCKF Feat = Red 
SLAM Feat = Orange 


S B Y) M A V & Th 2 a KS | Sensor Failures A Faak PO 


e This presentation: 


。 Introduced background on 
traditional VINS estimators 

e Discussed challenges in designing 
VINS algorithms 


e Presented summary of current 


open-sourced codebases, datasets, Please checkout OpenVINS! 
and evaluation tools - I 





| https://github.com/rpng/open vins | 


A : | . 。 | 
* Contact information: i https://docs.openvins.com/ 


e Patrick Geneva (@goldbattle github) 
* pgeneva@udel.edu 
。 https://pgeneva.com/ 





